#include "stdafx.h"

#include "math.h" 
#include "robot.h"

#define WIN_SIZE		32                   
#define PIXEL_LIMIT		10  // Group Boundary	
#define PIXEL_NUM		5   // Minimum Number of Pixels
#define ANGLE_LIMIT_X	2 
#define ANGLE_LIMIT_Y	2

#define GAME_RIGHT		0

#define M_PI 			3.14159265358979323846
 
         

/* "Predictor" Structure Definition */
Predictor::Predictor()
{
	int i;

	flag = count = 0;
	a = b = sum_x = sum_y = sum_xy = sum_xx = 0.0;
	for(i = 0;i < PREDICTOR_NUM;i++){
		save_x[i] = 0.0;
		save_y[i] = 0.0;
	}
}

void Predictor::add(double x, double y)
{
	double den;

	if(flag == 0 && count > 0)
		flag = 1;
	if(flag == 1 && count == PREDICTOR_NUM) //PREDICTOR_NUM 30
		flag = 2;
	if(count >= PREDICTOR_NUM)     
		count -= PREDICTOR_NUM;
	sum_x += (x-save_x[count]);
	sum_y += (y-save_y[count]);
	sum_xy += (x*y-save_x[count]*save_y[count]);
	sum_xx += (x*x-save_x[count]*save_x[count]);
	cur_x = x;
	cur_y = y;
	save_x[count] = x;
	save_y[count] = y;
	count++;

	if(flag == 2){
		den = PREDICTOR_NUM*sum_xx-sum_x*sum_x;
		a = (PREDICTOR_NUM*sum_xy-sum_x*sum_y)/den;
		b = (sum_y*sum_xx-sum_xy*sum_x)/den;
	}
	else if(flag == 1){
		den = count*sum_xx-sum_x*sum_x;
		a = (count*sum_xy-sum_x*sum_y)/den;
		b = (sum_y*sum_xx-sum_xy*sum_x)/den;
	}
}

void Predictor::clear()
{
	int i;

	flag = count = 0;
	sum_x = sum_y = sum_xy = sum_xx = 0.0;
	for(i = 0;i < PREDICTOR_NUM;i++){
		save_x[i] = 0.0;
		save_y[i] = 0.0;
	}
}

double Predictor::predict(double x)
{
	return(a*(cur_x+x)+b);
}

